package com.lucenten.collect.gnss.handler.tools.compute;

import cn.quevo.common.lang.DateFormatUtils;
import cn.quevo.common.lang.DateUtils;

import java.util.Calendar;
import java.util.Date;

/**
 * 龙格库塔算法
 *
 * @author July july_sky@foxmail.com
 * @version 1.0
 * @date 2019/5/3 23:20
 * @Copyright 东方浩星（北京）传媒科技有限公司版权所有.All Rights Reserved.
 */
public class RungeKutta {
    //牛顿引力常数与地球质量之积m^3/s^2
    private final double GM = 3.9860044 * (Math.pow(10, 14));
    //重力第二带谐系数
    private final double J2 = 1.0826257 * (Math.pow(10, -3));
    //地球长半径 m
    private final double ae = 6378136;
    //地球自转角速度 rad/s
    private final double w = 7.292115 * (Math.pow(10, -5));
    //创建x,y对象
    private static double[] x1;
    private static double[][] y1;

    public double[] getX1() {
        return x1;
    }

    public void setX1(double[] x1) {
        this.x1 = x1;
    }

    public double[][] getY1() {
        return y1;
    }

    public void setY1(double[][] y1) {
        this.y1 = y1;
    }

    public double[] function(double t, double[] y, double xls, double yls, double zls) {
        double r = Math.sqrt(y[0] * y[0] + y[1] * y[1] + y[2] * y[2]);  //距离 m
        // Xls=(-0.372529029846E-08)*10^3
        // Yls=(-0.931322574615E-09)*10^3
        // Zls=(-0.186264514923E-08)*10^3
        // Xls=0
        // Yls=0.279396772385*(10^(-5))
        // Zls=0

        double[] dy = new double[6];    // a column vector
        dy[0] = y[3];
        dy[1] = y[4];
        dy[2] = y[5];
        double c1 = -GM / (Math.pow(r, 3));
        double c2 = 1.5 * J2 * GM * Math.pow(ae, 2) / Math.pow(r, 5);
        double c3 = (1 - 5 * Math.pow(y[2], 2) / Math.pow(r, 2));
        double c4 = Math.pow(w, 2);
        dy[3] = c1 * y[0] + c2 * y[0] * c3 + xls + c4 * y[0] + 2 * w * y[4];
        dy[4] = c1 * y[1] + c2 * y[1] * c3 + yls + c4 * y[1] - 2 * w * y[3];
        dy[5] = c1 * y[2] + c2 * y[2] * (3 - 5 * Math.pow(y[2], 2) / Math.pow(r, 2)) + zls;
//        dy[3] = -GM / (Math.pow(r, 3)) * y[0] + 1.5 * J2 * GM * Math.pow(ae, 2) / Math.pow(r, 5) * y[0] * (1 - 5 * Math.pow(y[2], 2) / Math.pow(r, 2)) + xls + Math.pow(w, 2) * y[0] + 2 * w * y[4];
//        dy[4] = -GM / (Math.pow(r, 3)) * y[1] + 1.5 * J2 * GM * Math.pow(ae, 2) / Math.pow(r, 5) * y[1] * (1 - 5 * Math.pow(y[2], 2) / Math.pow(r, 2)) + yls + Math.pow(w, 2) * y[1] - 2 * w * y[3];
//        dy[5] = -GM / (Math.pow(r, 3)) * y[2] + 1.5 * J2 * GM * Math.pow(ae, 2) / Math.pow(r, 5) * y[2] * (3 - 5 * Math.pow(y[2], 2) / Math.pow(r, 2)) + zls;
        return dy;
    }

    public void rungeKutta(long beginTime, long endTime, int interval, double[] vals, double xls, double yls, double zls) {
        double[] k1, k2, k3, k4;
        int n = (int) Math.abs(Math.floor(endTime - beginTime) / interval);

        //求步数
        //时间起点
        double[] x = new double[n + 1];
        x[0] = beginTime;
        //赋初值，可以是向量，但是要注意维数
        /*double[][] y = new double[6][(int) (n + 1)];
        for (int j = 0; j < y0[0].length; j++) {
            y[j][0] = y0[0][j]; //y六行一列数组
        }*/
        double[][] y = new double[n + 1][6];
        double[] tmp = new double[6];
        for (int i = 0; i < vals.length; i++) {
            y[0][i] = vals[i];
            tmp[i] = vals[i];
        }

        FactorialCalculator calculator = new FactorialCalculator();
        for (int i = 0; i < n; i++) {
            x[i + 1] = x[i] + interval;
            k1 = this.function(x[i], tmp, xls, yls, zls);
            k2 = this.function(x[i] + interval / 2, calculator.plus(tmp, calculator.multiply(k1, interval * 0.5d)), xls, yls, zls);
            k3 = this.function(x[i] + interval / 2, calculator.plus(tmp, calculator.multiply(k2, interval * 0.5d)), xls, yls, zls);
            k4 = this.function(x[i] + interval, calculator.plus(tmp, calculator.multiply(k3, interval)), xls, yls, zls);
            double[] ksum = calculator.plus(calculator.plus(calculator.plus(k1, calculator.multiply(k2, 2)), calculator.multiply(k3, 2)), k4);
            double[] endnum = calculator.plus(tmp, calculator.multiply(ksum, interval / 6d));
            tmp = new double[6];
            for (int j = 0; j < endnum.length; j++) {
                y[i + 1][j] = endnum[j];
                tmp[j] = endnum[j];
            }
        }
        this.x1 = x;
        this.y1 = y;
    }
}